#include "bsp_can.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "esp_err.h"
#include "esp_log.h"
#include "driver/twai.h"
#include "driver/twai.h"
#include "app_data.h"
#include "bsp_sleep.h"

static twai_timing_config_t t_config_250k = TWAI_TIMING_CONFIG_250KBITS();
static twai_timing_config_t t_config_500k = TWAI_TIMING_CONFIG_500KBITS();
static twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
static twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(TX_GPIO_NUM, RX_GPIO_NUM, TWAI_MODE_NORMAL);
twai_message_t OBD_message = {.identifier = ID_OBD_Standard, .data_length_code = 8, .ss = 1, .data = {0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};

uint8_t can_mode = 0;
//1:250K 标准帧
//2:250K 扩展帧
//3:500K 标准帧
//4:500K 扩展帧

static const char *TAG = "BSP_CAN";

static esp_err_t bsp_can_reinstall(uint8_t can_500k_flag);
static void bsp_can_install(uint8_t can_mode);

void bsp_can_task(void *arg)
{

    uint8_t tx_num = 0, rx_time_num = 0, can_500k_flag = 0;
    uint8_t OBD_PID[5] = {0x05, 0x0d, 0x2f, 0x0c, 0x42}; //水温，转速，油量，车速，电压
    esp_err_t twai_receive_action;
    twai_message_t rx_msg;
    uint8_t Identifier[4] = {0};
    can_mode = 4;
    if ((can_mode == 0) || (can_mode > 4))
        can_mode = 0;
    if (can_mode > 2)
    {
        can_500k_flag = 1;
    }
    bsp_can_install(can_mode);
    if (can_mode != 0) //can_mode 有被存储，优先识别一下
    {
        while (rx_time_num < 30)
        {
            tx_num++;
            if (tx_num == 2)
                tx_num = 0;
            OBD_message.data[2] = OBD_PID[tx_num];
            twai_transmit(&OBD_message, 50);
            vTaskDelay(50 / portTICK_PERIOD_MS);
            twai_receive_action = twai_receive(&rx_msg, 1);
            if (twai_receive_action == ESP_ERR_TIMEOUT)
            {
                rx_time_num++;
                if (rx_time_num == 30)
                    can_mode = 0;
            }
            else if ((twai_receive_action == ESP_OK) && (rx_msg.data_length_code != 0))
            {
                if ((rx_msg.identifier == 0x7e8) || (rx_msg.identifier == 0x7e9) || (rx_msg.identifier > 0x18DAF000))
                {
                    if ((rx_msg.data[0] == 0x03 || rx_msg.data[0] == 0x04) && (rx_msg.data[1] == 0x41))
                    {
                        Identifier[0] = (uint8_t)(rx_msg.identifier >> 24);
                        Identifier[1] = (uint8_t)(rx_msg.identifier >> 16);
                        Identifier[2] = (uint8_t)(rx_msg.identifier >> 8);
                        Identifier[3] = (uint8_t)(rx_msg.identifier);
                        rx_time_num = 100;
                        if (rx_msg.extd)
                        {
                            esp_log_buffer_hex("Identifier", &Identifier, 4);
                        }
                        else
                        {
                            esp_log_buffer_hex("Identifier", &Identifier[2], 2);
                        }
                    }
                }
            }
        }
        rx_time_num = 0;
        tx_num = 0;
    }
    if (can_mode == 0) //can_mode 没有被存储或者上一步识别失败，再次识别一些其他帧
    {
        while (rx_time_num < 60)
        {
            tx_num++;
            if (tx_num == 2)
                tx_num = 0;
            OBD_message.data[2] = OBD_PID[tx_num];
            OBD_message.extd = 1;
            OBD_message.identifier = ID_OBD_Extended;
            twai_transmit(&OBD_message, 50);
            OBD_message.extd = 0;
            OBD_message.identifier = ID_OBD_Standard;
            twai_transmit(&OBD_message, 50);
            vTaskDelay(50 / portTICK_PERIOD_MS);
            twai_receive_action = twai_receive(&rx_msg, 1);
            if (twai_receive_action == ESP_ERR_TIMEOUT)
            {
                rx_time_num++;
            }
            else if ((twai_receive_action == ESP_OK) && (rx_msg.data_length_code != 0))
            {
                if ((rx_msg.identifier == 0x7e8) || (rx_msg.identifier == 0x7e9) || (rx_msg.identifier > 0x18DAF000))
                {
                    if ((rx_msg.data[0] == 0x03 || rx_msg.data[0] == 0x04) && (rx_msg.data[1] == 0x41))
                    {
                        Identifier[0] = (uint8_t)(rx_msg.identifier >> 24);
                        Identifier[1] = (uint8_t)(rx_msg.identifier >> 16);
                        Identifier[2] = (uint8_t)(rx_msg.identifier >> 8);
                        Identifier[3] = (uint8_t)(rx_msg.identifier);
                        //1:250K 标准帧
                        //2:250K 扩展帧
                        //3:500K 标准帧
                        //4:500K 扩展帧
                        if (can_500k_flag == 1)
                        {
                            if (rx_msg.extd)
                            {
                                OBD_message.extd = 1;
                                OBD_message.identifier = ID_OBD_Extended;
                                can_mode = 4;
                            }
                            else
                            {
                                OBD_message.extd = 0;
                                OBD_message.identifier = ID_OBD_Standard;
                                can_mode = 3;
                            }
                        }
                        else
                        {
                            if (rx_msg.extd)
                            {
                                OBD_message.extd = 1;
                                OBD_message.identifier = ID_OBD_Extended;
                                can_mode = 2;
                            }
                            else
                            {
                                OBD_message.extd = 0;
                                OBD_message.identifier = ID_OBD_Standard;
                                can_mode = 1;
                            }
                        }
                        rx_time_num = 100;
                        if (rx_msg.extd)
                        {
                            esp_log_buffer_hex("Identifier", &Identifier, 4);
                        }
                        else
                        {
                            esp_log_buffer_hex("Identifier", &Identifier[2], 2);
                        }
                    }
                }
            }
        }
        rx_time_num = 0;
        tx_num = 0;
    }
    if (can_mode == 0) //can_mode 上一步识别失败，再次识别一些其他频率和帧
    {
        bsp_can_reinstall(can_500k_flag);
        can_500k_flag = can_500k_flag ? 0 : 1;
        while (rx_time_num < 60)
        {
            tx_num++;
            if (tx_num == 2)
                tx_num = 0;
            OBD_message.data[2] = OBD_PID[tx_num];
            OBD_message.extd = 1;
            OBD_message.identifier = ID_OBD_Extended;
            twai_transmit(&OBD_message, 50);
            OBD_message.extd = 0;
            OBD_message.identifier = ID_OBD_Standard;
            twai_transmit(&OBD_message, 50);
            vTaskDelay(50 / portTICK_PERIOD_MS);
            twai_receive_action = twai_receive(&rx_msg, 1);
            if (twai_receive_action == ESP_ERR_TIMEOUT)
            {
                rx_time_num++;
            }
            else if ((twai_receive_action == ESP_OK) && (rx_msg.data_length_code != 0))
            {
                if ((rx_msg.identifier == 0x7e8) || (rx_msg.identifier == 0x7e9) || (rx_msg.identifier > 0x18DAF000))
                {
                    if ((rx_msg.data[0] == 0x03 || rx_msg.data[0] == 0x04) && (rx_msg.data[1] == 0x41))
                    {
                        Identifier[0] = (uint8_t)(rx_msg.identifier >> 24);
                        Identifier[1] = (uint8_t)(rx_msg.identifier >> 16);
                        Identifier[2] = (uint8_t)(rx_msg.identifier >> 8);
                        Identifier[3] = (uint8_t)(rx_msg.identifier);
                        //1:250K 标准帧
                        //2:250K 扩展帧
                        //3:500K 标准帧
                        //4:500K 扩展帧
                        if (can_500k_flag == 1)
                        {
                            if (rx_msg.extd)
                            {
                                OBD_message.extd = 1;
                                OBD_message.identifier = ID_OBD_Extended;
                                can_mode = 4;
                            }
                            else
                            {
                                OBD_message.extd = 0;
                                OBD_message.identifier = ID_OBD_Standard;
                                can_mode = 3;
                            }
                        }
                        else
                        {
                            if (rx_msg.extd)
                            {
                                OBD_message.extd = 1;
                                OBD_message.identifier = ID_OBD_Extended;
                                can_mode = 2;
                            }
                            else
                            {
                                OBD_message.extd = 0;
                                OBD_message.identifier = ID_OBD_Standard;
                                can_mode = 1;
                            }
                        }
                        rx_time_num = 100;
                        if (rx_msg.extd)
                        {
                            esp_log_buffer_hex("Identifier", &Identifier, 4);
                        }
                        else
                        {
                            esp_log_buffer_hex("Identifier", &Identifier[2], 2);
                        }
                    }
                }
            }
        }
        rx_time_num = 0;
        tx_num = 0;
    }
    while (1)
    {
        ESP_LOGI(TAG, "can_mode=%d", can_mode);
        tx_num++;
        if (tx_num == 2)
            tx_num = 0;
        OBD_message.data[2] = OBD_PID[tx_num];
        twai_transmit(&OBD_message, 50);
        vTaskDelay(50 / portTICK_RATE_MS);
        twai_receive_action = twai_receive(&rx_msg, 1);
        if (twai_receive_action == ESP_ERR_TIMEOUT)
        {
            rx_time_num++;
            if (rx_time_num > 60)
            {
                rx_time_num = 60;
                app_data.obd.enable = 0;
            }
        }
        else if ((twai_receive_action == ESP_OK) && (rx_msg.data_length_code != 0))
        {
            if ((rx_msg.identifier == 0x7e8) || (rx_msg.identifier == 0x7e9) || (rx_msg.identifier > 0x18DAF000))
            {
                if ((rx_msg.data[0] == 0x03 || rx_msg.data[0] == 0x04) && (rx_msg.data[1] == 0x41))
                {
                    app_data.obd.enable = 1;
                    rx_time_num = 0;
                    switch (rx_msg.data[2])
                    {
                    case 0x05:
                        app_data.obd.water = (int16_t)rx_msg.data[3] - 40;
                        break;
                    case 0x2f:
                        app_data.obd.gas = (uint8_t)((uint16_t)rx_msg.data[3] * 100 / 255);
                        break;
                    case 0x0d:
                        app_data.obd.speed = rx_msg.data[3];
                        break;
                    case 0x42:
                        app_data.obd.voltage = (uint8_t)(((uint16_t)rx_msg.data[3] * 256 + (uint16_t)rx_msg.data[4]) / 100);
                        break;
                    default:
                        break;
                    }
                }
            }
        }
    }
}

static esp_err_t bsp_can_reinstall(uint8_t can_500k_flag)
{
    esp_err_t err;

    err = twai_clear_transmit_queue();
    if (err != ESP_OK)
    {
        ESP_LOGI(TAG, "clear_transmit ERR");
        goto ret;
    }

    err = twai_clear_receive_queue();
    if (err != ESP_OK)
    {
        ESP_LOGI(TAG, "clear_receive ERR");
        goto ret;
    }

    err = twai_stop();
    if (err != ESP_OK)
    {
        ESP_LOGI(TAG, "twai_stop ERR");
        goto ret;
    }

    err = twai_driver_uninstall();
    if (err != ESP_OK)
    {
        ESP_LOGI(TAG, "twai_driver_uninstall ERR");
        goto ret;
    }
    ESP_LOGI(TAG, "Driver uninstalled");

    if (can_500k_flag)
    {
        err = twai_driver_install(&g_config, &t_config_250k, &f_config);
        if (err != ESP_OK)
        {
            ESP_LOGI(TAG, "twai_driver_install ERR");
            goto ret;
        }
        app_data.can_type = 1;
        bsp_sleep_save_data(SAVE_CAN);
    }
    else
    {
        err = twai_driver_install(&g_config, &t_config_500k, &f_config);
        if (err != ESP_OK)
        {
            ESP_LOGI(TAG, "twai_driver_install ERR");
            goto ret;
        }
        app_data.can_type = 2;
        bsp_sleep_save_data(SAVE_CAN);
    }

    err = twai_start();
    if (err != ESP_OK)
    {
        ESP_LOGI(TAG, "twai_start ERR");
        goto ret;
    }
    ESP_LOGI(TAG, "Driver installed");

    return ESP_OK;

ret:
    ESP_LOGI(TAG, "twai_driver_install ERR");
    return ESP_FAIL;
}

static void bsp_can_install(uint8_t can_mode)
{
    switch (can_mode)
    {
    case 1: //250K 标准帧
    case 2: //250K 扩展帧
        if (can_mode == 1)
        {
            OBD_message.extd = 1;
            OBD_message.identifier = ID_OBD_Extended;
        }
        else
        {
            OBD_message.extd = 0;
            OBD_message.identifier = ID_OBD_Standard;
        }
        ESP_ERROR_CHECK(twai_driver_install(&g_config, &t_config_250k, &f_config));
        app_data.can_type = 1;
        bsp_sleep_save_data(SAVE_CAN);
        break;
    case 3: //500K 标准帧
    case 4: //500K 扩展帧
        if (can_mode == 3)
        {
            OBD_message.extd = 1;
            OBD_message.identifier = ID_OBD_Extended;
        }
        else
        {
            OBD_message.extd = 0;
            OBD_message.identifier = ID_OBD_Standard;
        }
        ESP_ERROR_CHECK(twai_driver_install(&g_config, &t_config_500k, &f_config));
        app_data.can_type = 2;
        bsp_sleep_save_data(SAVE_CAN);
        break;
    default:
        ESP_ERROR_CHECK(twai_driver_install(&g_config, &t_config_250k, &f_config));
        app_data.can_type = 1;
        bsp_sleep_save_data(SAVE_CAN);
        break;
    }
    ESP_ERROR_CHECK(twai_start());
}
